/*
===========================================================================
This source file is part of DyLab (Dynamic Laboratory)
For the latest info, see http://dylab-modules.googlecode.com

Copyright (c) 2006-2008 Lukas Krejci
(krejci.lukas@volny.cz)

This file is part of DyLab.

    DyLab is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    DyLab is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with DyLab.  If not, see <http://www.gnu.org/licenses/>.
===========================================================================
*/

//===========================================================================================
#include "Versatile_ModelTechnique.h"
#include <dylab/simulation/Simulation.h>
#include <dylab/utils/exceptions/EInvalidState.h>

//===========================================================================================
const dylab::String dylab::Versatile_ModelTechnique::COMPONENT_NAME = "versatile";

//===========================================================================================
dylab::Versatile_ModelTechnique::DistanceConstraint::DistanceConstraint(Edge * _edge) 
: mEdge(_edge), mOriginalDistance(0.0f), mStiffness(1.0f), mDamping(0.001f)
, mPlasticEnabled(false), mPlasticYieldEnergy(0.5f), mPlasticMaxEnergy(256.0f), mPlasticCreep(0.5f), mPlasticCurrEnergy(0.0f)
{   
    // check pointer to the edge
    DYLAB_ASSERT(mEdge != NULL);

    // store the original edge length
    mOriginalDistance = mEdge->getGeometry().computeLength();
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::DistanceConstraint::performComputation()
{
    // store the mass point pointers
    ComModelStructure::IMassPoints::MassPoint * mp0 = mEdge->getMassPoint(EdgeGeometry3::V_0); 
    ComModelStructure::IMassPoints::MassPoint * mp1 = mEdge->getMassPoint(EdgeGeometry3::V_1);

    // skip, if all nodes are fixed
    if (mp0->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED) && mp1->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED))
        return;

    // calculate force on these points based on distance constraint
    Vector3 nodeDiff = mp1->currState().position - mp0->currState().position;
    real_t distance = nodeDiff.length(); // current distance

    if (distance == 0)
        throw EInvalidState("Zero distance is not allowed."); // TODO: only trace?

    // energy generated by distance constraint
    real_t E = (distance - mOriginalDistance) / mOriginalDistance;  

    //
    Vector3 eN2 = nodeDiff * (1.0f / distance * mOriginalDistance);
    Vector3 eN1 = -eN2; 
    real_t forceMag = -1.0f * mStiffness * E - mDamping * (eN1.dot(mp0->currState().velocity) + eN2.dot(mp1->currState().velocity));
    Vector3 fN1 = eN1;
    fN1 *= forceMag;

    //
    if (mPlasticEnabled)
    {
        // TODO: je treba prubezne odstranoat tetrahedrony nesplnujici kriteria stability (napr. min distance, area, volume)
        real_t energy = 0.5f * mStiffness * E * E;  // compute distance preserving energy

        if (abs(energy) > mPlasticYieldEnergy)
            mPlasticCurrEnergy += mPlasticCreep * energy;    

        if (abs(mPlasticCurrEnergy) > mPlasticMaxEnergy) 
            mPlasticCurrEnergy = mPlasticMaxEnergy; 

        // compute plastic force
        real_t EPlastic = sqrt(2.0f * mPlasticCurrEnergy / mStiffness);
        real_t distancePlastic = EPlastic * mOriginalDistance + mOriginalDistance;
        nodeDiff.normalise();
        Vector3 eN2Plastic = nodeDiff * distancePlastic * (1.0f / distancePlastic * mOriginalDistance);
        Vector3 eN1Plastic = -eN2Plastic; 
        real_t forceMagPlastic = -1.0f * mStiffness * EPlastic - mDamping * (eN1Plastic.dot(mp0->currState().velocity) + eN2Plastic.dot(mp1->currState().velocity));
        Vector3 fN1Plastic = eN1Plastic;
        fN1Plastic *= forceMagPlastic;

        //
        fN1 = fN1 + fN1Plastic;
    }

    //
    mp0->currState().force += fN1;
    mp1->currState().force -= fN1;
}

//===========================================================================================
dylab::Versatile_ModelTechnique::AreaConstraint::AreaConstraint(Triangle * _triangle)
: mTriangle(_triangle)
{ 
    // check pointer the triangle
    DYLAB_ASSERT(mTriangle != NULL);

    // compute original area
    mOriginalArea =  mTriangle->getGeometry().computeArea();

}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::AreaConstraint::performComputation()
{
    // store the mass point pointers
    ComModelStructure::IMassPoints::MassPoint * mp0 = mTriangle->getMassPoint(TriangleGeometry3::V_0); 
    ComModelStructure::IMassPoints::MassPoint * mp1 = mTriangle->getMassPoint(TriangleGeometry3::V_1);
    ComModelStructure::IMassPoints::MassPoint * mp2 = mTriangle->getMassPoint(TriangleGeometry3::V_2);

    // skip, if all nodes are fixed
    if (mp0->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED) && mp1->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED) && 
        mp2->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED))
        return;

    Vector3 p0 = mp0->currState().position;
    Vector3 p1 = mp1->currState().position;
    Vector3 p2 = mp2->currState().position;

    // calculate forces on these nodes based on volume constraint
    Vector3 p10 = p1 - p0;
    Vector3 p20 = p2 - p0;

    real_t alfa_1 = p10.y() * p20.z() - p10.z() * p20.y();
    real_t alfa_2 = p10.z() * p20.x() - p10.x() * p20.z();
    real_t alfa_3 = p10.x() * p20.y() - p10.y() * p20.x();
    real_t alfa = sqrt(alfa_1 * alfa_1 + alfa_2 * alfa_2 + alfa_3 * alfa_3);

    real_t area = alfa / DYLAB_R(2); // current area    
    if (area == 0)
        throw EInvalidState("Zero area is not allowed.");

    // contraint energy CA: -1 * kA * CA / 2 * alfa * A0
    real_t E = -1.0f * mStiffness * (area - mOriginalArea) / (2.0f * mOriginalArea * mOriginalArea * alfa);

    // helper points
    Vector3 p21 = p2 - p1;
    Vector3 p12 = p1 - p2;
    Vector3 p02 = p0 - p2;

    // compute forces on mass points
    Vector3 fN0, fN1, fN2;
    
    fN0.x() = E * (alfa_2 * p21.z() + alfa_3 * p12.y());
    fN0.y() = E * (alfa_1 * p12.z() + alfa_3 * p21.x());
    fN0.z() = E * (alfa_1 * p21.y() + alfa_2 * p12.x());

    fN1.x() = E * (alfa_2 * p02.z() + alfa_3 * p20.y());
    fN1.y() = E * (alfa_1 * p20.z() + alfa_3 * p02.x());
    fN1.z() = E * (alfa_1 * p02.y() + alfa_2 * p20.x());

    fN2 = -(fN0 + fN1); // sum of all inner forces must be zero

    // apply the computed forces on the corresponding nodes
    mp0->currState().force += fN0;
    mp1->currState().force += fN1;
    mp2->currState().force += fN2;
}

//===========================================================================================
dylab::Versatile_ModelTechnique::VolumeConstraint::VolumeConstraint(Tetrahedron * _tetrahedron)
: mTetrahedron(_tetrahedron)
{ 
    // check pointer to the tetrahedron
    DYLAB_ASSERT(mTetrahedron != NULL);

    // compute original volume
    mOriginalVolume = mTetrahedron->getGeometry().computeVolume(); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::VolumeConstraint::performComputation()
{
    // store the mass point pointers
    ComModelStructure::IMassPoints::MassPoint * mp0 = mTetrahedron->getMassPoint(TetrahedronGeometry::V_0); 
    ComModelStructure::IMassPoints::MassPoint * mp1 = mTetrahedron->getMassPoint(TetrahedronGeometry::V_1);
    ComModelStructure::IMassPoints::MassPoint * mp2 = mTetrahedron->getMassPoint(TetrahedronGeometry::V_2);
    ComModelStructure::IMassPoints::MassPoint * mp3 = mTetrahedron->getMassPoint(TetrahedronGeometry::V_3);

    // skip, if all nodes are fixed
    if (mp0->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED) && mp1->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED) && 
        mp2->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED) && mp3->flags().test(ComModelStructure::IMassPoints::MassPoint::F_FIXED))
        return;

    Vector3 p0 = mp0->currState().position;
    Vector3 p1 = mp1->currState().position;
    Vector3 p2 = mp2->currState().position;
    Vector3 p3 = mp3->currState().position;

    // calculate forces on these nodes based on volume constraint
    Vector3 p10 = p1 - p0;
    Vector3 p20 = p2 - p0;
    Vector3 p30 = p3 - p0;

    real_t d1 = p20.y() * p30.z() - p20.z() * p30.y();
    real_t d2 = p20.z() * p30.x() - p20.x() * p30.z();
    real_t d3 = p20.x() * p30.y() - p20.y() * p30.x();
    real_t delta = p10.x() * d1 + p10.y() * d2 + p10.z() * d3;
    
    real_t volume = delta / DYLAB_R(6); // current volume   
    if (volume == 0)
        throw EInvalidState("Zero volume is not allowed.");

    // contraint energy CV: -1 * kV * CV / 6 * V0
    real_t E = -1.0f * mStiffness * (volume - mOriginalVolume) / (6.0f * mOriginalVolume * mOriginalVolume);
    
    // helper points
    Vector3 p32 = p3 - p2;
    Vector3 p23 = p2 - p3;    
    Vector3 p03 = p0 - p3;
    Vector3 p02 = p0 - p2;

    // compute forces on mass points
    Vector3 fN0, fN1, fN2, fN3;
    
    fN0.x() = E * (p10.y() * p32.z() + p10.z() * p23.y() - d1);
    fN0.y() = E * (p10.x() * p23.z() + p10.z() * p32.x() - d2);
    fN0.z() = E * (p10.x() * p32.y() + p10.y() * p23.x() - d3);

    fN1.x() = E * d1;
    fN1.y() = E * d2;
    fN1.z() = E * d3;

    fN2.x() = E * (p10.y() * p03.z() + p10.z() * p30.y());
    fN2.y() = E * (p10.x() * p30.z() + p10.z() * p03.x());
    fN2.z() = E * (p10.x() * p03.y() + p10.y() * p30.x());    

    fN3 = -(fN0 + fN1 + fN2); // sum of all inner forces must be zero

    // apply the computed forces on the corresponding nodes
    mp0->currState().force += fN0;
    mp1->currState().force += fN1;
    mp2->currState().force += fN2;
    mp3->currState().force += fN3;
}

//===========================================================================================
dylab::Versatile_ModelTechnique::Versatile_ModelTechnique()
: ComModelTechnique(COMPONENT_NAME), mParentSimObject(NULL), mModel(NULL)
, mDistanceConstraintsEnabled(false), mVolumeConstraintsEnabled(false), mAreaConstraintsEnabled(false) 
, mDistanceConstraints(true), mAreaConstraints(true), mVolumeConstraints(true)
{
}
//-----------------------------------------------------------------------------
dylab::Versatile_ModelTechnique::~Versatile_ModelTechnique()
{
}
//-----------------------------------------------------------------------------
dylab::String dylab::Versatile_ModelTechnique::getDescription() const
{
    uint32_t d = mDistanceConstraintsEnabled ? mDistanceConstraints.getCount() : 0;
    uint32_t a = mAreaConstraintsEnabled ? mAreaConstraints.getCount() : 0;
    uint32_t v = mVolumeConstraintsEnabled ? mVolumeConstraints.getCount() : 0;
    return String::fmt("%1%(d=%2%,a=%3%,v=%4%)", getFullName(), d, a, v);
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::initialize(SimObject * _parentSimObject)
{
    DYLAB_ASSERT(_parentSimObject != NULL);

    // avoid double initialization
    // TODO - delete old massPoints structures
    if (mParentSimObject != NULL)
        throw EInvalidState("Second initialization is not allowed.");

    // store the parent simulation object
    mParentSimObject = _parentSimObject;    

    // store the tetramesh massPoints
    mModel = mParentSimObject->getModelStructure();    

    // lock massPoints
    ComModelStructure::ScopedExLock structure(mModel);

    // retrieve relevant interfaces
    ComModelStructure::ITetrahedralVolume * tetraVolume = structure->getTetrahedralVolumeInterface();

    // zero all forces
    for (ComModelStructure::IMassPoints::MassPointMap::Iterator mp(structure->getMassPointsInterface()->massPointIterator()); mp.isValid(); mp++) 
        mp->currState().force = Vector3::ZERO;

    // create distance constraints
    for (ComModelStructure::ITetrahedralVolume::EdgeMap::Iterator edge(tetraVolume->edgeIterator()); edge.isValid(); edge++)    
        mDistanceConstraints.insert(~edge, new DistanceConstraint(edge));
    

    // create area constraints
    for (ComModelStructure::ITetrahedralVolume::TriangleMap::Iterator triangle(tetraVolume->triangleIterator()); triangle.isValid(); triangle++)            
        if (triangle->isBoundary()) // only for boundary triangles
            mAreaConstraints.insert(~triangle, new AreaConstraint(triangle));           

    // create volume constraints
    for (ComModelStructure::ITetrahedralVolume::TetrahedronMap::Iterator tetra(tetraVolume->tetrahedronIterator()); tetra.isValid(); tetra++)
        mVolumeConstraints.insert(~tetra, new VolumeConstraint(tetra));    
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::loadFromPropertyTree(const PropertyTree & _pt)
{
    // distance constraints
    const PropertyTree::Node * distance = _pt.findNode("distance");
    if (distance != NULL)
    {
        setDistanceConstraintsEnabled(distance->getValue().parse<bool>());
        setDistanceStiffness(distance->getChildNodeValueT("stiffness", DYLAB_R(100)));
        setDistanceDamping(distance->getChildNodeValueT("damping", DYLAB_R(0.001)));
    }

    // area constraints
    const PropertyTree::Node * area = _pt.findNode("area");
    if (area != NULL)
    {
        setAreaConstraintsEnabled(area->getValue().parse<bool>());
        setAreaStiffness(area->getChildNodeValueT("stiffness", DYLAB_R(100)));
    }

    // volume constraints
    const PropertyTree::Node * volume = _pt.findNode("volume");
    if (volume != NULL)
    {
        setVolumeConstraintsEnabled(volume->getValue().parse<bool>());
        setVolumeStiffness(volume->getChildNodeValueT("stiffness", DYLAB_R(100)));
    }

    // plastic (experimental)
    const PropertyTree::Node * plastic = _pt.findNode("plastic");
    if (plastic != NULL)
    {
        setPlasticEnabled(plastic->getValue().parse<bool>());
        setPlasticYieldEnergy(plastic->getChildNodeValueT("yieldEnergy", DYLAB_R(0.5)));
        setPlasticMaxEnergy(plastic->getChildNodeValueT("maxEnergy", DYLAB_R(512)));
        setPlasticCreep(plastic->getChildNodeValueT("creep", DYLAB_R(0.01)));
    }
}
//-----------------------------------------------------------------------------
dylab::SimObject * dylab::Versatile_ModelTechnique::getParentSimObject()
{
    return mParentSimObject;
}
//-----------------------------------------------------------------------------
const dylab::SimObject * dylab::Versatile_ModelTechnique::getParentSimObject() const
{
    return mParentSimObject;
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::performComputation()
{    
    // lock the structure and retrieve mass points interface
    ComModelStructure::IMassPoints::ScopedExLock massPoints(mModel);

    // then, compute forces that are generated by distance constraints, if enabled
    if (mDistanceConstraintsEnabled)
        for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
            dc->performComputation();

    // then, compute forces that are generated by area constraints, if enabled
    if (mAreaConstraintsEnabled)
        for (AreaConstraints::Iterator ac(mAreaConstraints); ac.isValid(); ac++)
            ac->performComputation();

    // then, compute forces that are generated by volume constraints, if enabled
    if (mVolumeConstraintsEnabled)
        for (VolumeConstraints::Iterator vc(mVolumeConstraints); vc.isValid(); vc++)
            vc->performComputation();
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setDistanceStiffness(real_t _stiffness) 
{ 
    for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
        dc->setStiffness(_stiffness); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setDistanceDamping(real_t _damping) 
{ 
    for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
        dc->setDamping(_damping); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setPlasticEnabled(bool _enable) 
{ 
    for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
        dc->setPlasticEnabled(_enable); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setPlasticYieldEnergy(real_t _plasticYieldEnergy) 
{ 
    for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
        dc->setPlasticYieldEnergy(_plasticYieldEnergy); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setPlasticMaxEnergy(real_t _plasticMaxEnergy) 
{ 
    for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
        dc->setPlasticMaxEnergy(_plasticMaxEnergy); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setPlasticCreep(real_t _plasticCreep) 
{ 
    for (DistanceConstraints::Iterator dc(mDistanceConstraints); dc.isValid(); dc++)
        dc->setPlasticCreep(_plasticCreep); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setAreaStiffness(real_t _stiffness) 
{ 
    for (AreaConstraints::Iterator ac(mAreaConstraints); ac.isValid(); ac++)
        ac->setStiffness(_stiffness); 
}
//-----------------------------------------------------------------------------
void dylab::Versatile_ModelTechnique::setVolumeStiffness(real_t _stiffness) 
{ 
    for (VolumeConstraints::Iterator vc(mVolumeConstraints); vc.isValid(); vc++)
        vc->setStiffness(_stiffness); 
}
//-----------------------------------------------------------------------------